/*
wheel.c
offer AGV wheel run control function
designed by lunanting

--------------------------------
2020-03-30 21:32:01
create file
--------------------------------
*/

#include "wheel.h"
#include "stdlib.h"
#include "../../agv/io.h"
#include "math.h"
#include "gyro.h"
#include "linux_lidar.h"

static int wheel_locker = 0;

static PANS_handler wheel_handler[2]={0};
static float wheel_cmd_speed[2] = {0};
static WHEEL_object agv_wheel={0};
static aos_mutex_t wheel_mux;

#define MOTOR_num 2

void xp_wheel_distance_calc_thread(void *arg);
static void xp_wheel_run_check_thread(void *arg);

/*
config wheel motor driver
*/
void xp_wheel_config(void){
    PANS_config *conf=&wheel_handler[0].config;
    int speed=xp_wheel_get_speed(WHEEL_line_speed);

    agv_wheel.init=0;
    agv_wheel.normal_speed=speed;
    agv_wheel.io[0]=WHEEL_io_left_event;
    agv_wheel.io[1]=WHEEL_io_right_event;
    //485端口配置
    wheel_handler[0].port=2;
    wheel_handler[0].speed=115200;
    wheel_handler[0].slave_id=WHEEL_left_addr;
    wheel_handler[1].port=2;
    wheel_handler[1].speed=115200;
    wheel_handler[1].slave_id=WHEEL_right_addr;

    //电机基础配置
    conf->home_offset=0;
    conf->max_limit=0;
    conf->min_limit=-0;

    //电机的速度，加速度，减速度配置
    conf->speed[0]=speed/2;
    conf->speed[1]=speed;
    conf->speed[2]=1000;
    conf->speed[3]=3000;
    conf->speed[4]=6000;
    conf->acc[0]=WHEEL_acc_dec;
    conf->acc[1]=WHEEL_acc_dec;
    conf->acc[2]=1500;
    conf->acc[3]=1500;
    conf->acc[4]=1500;
    conf->dec[0]=WHEEL_acc_dec;
    conf->dec[1]=WHEEL_acc_dec;
    conf->dec[2]=1500;
    conf->dec[3]=1500;
    conf->dec[4]=500;
    wheel_handler[1].config.speed[0]=conf->speed[0];
    wheel_handler[1].config.speed[1]=conf->speed[1];
    wheel_handler[1].config.speed[2]=conf->speed[2];
    wheel_handler[1].config.speed[3]=conf->speed[3];
    wheel_handler[1].config.speed[4]=conf->speed[4];

    wheel_handler[1].config.acc[0]=conf->acc[0];
    wheel_handler[1].config.acc[1]=conf->acc[1];
    wheel_handler[1].config.acc[2]=conf->acc[2];
    wheel_handler[1].config.acc[3]=conf->acc[3];
    wheel_handler[1].config.acc[4]=conf->acc[4];

    wheel_handler[1].config.dec[0]=conf->dec[0];
    wheel_handler[1].config.dec[1]=conf->dec[1];
    wheel_handler[1].config.dec[2]=conf->dec[2];
    wheel_handler[1].config.dec[3]=conf->dec[3];
    wheel_handler[1].config.dec[4]=conf->dec[4];
}

/*
    用于不重启恢复时重新配置轮子
*/
int xp_wheel_recover(u8 port,int speed,u8 slave_id){
    int sta=0;

    xp_wheel_config();
    wheel_handler[0].port=port;
    wheel_handler[0].speed=speed;
    // wheel_handler[0].slave_id=slave_id;
    wheel_handler[1].port=port;
    wheel_handler[1].speed=speed;
    // wheel_handler[1].slave_id=slave_id+1;
    agv_wheel.motor[0]=&wheel_handler[0];
    agv_wheel.motor[1]=&wheel_handler[1];
    agv_wheel.turn=wheel_middle;
	
    sta=xp_pans_init(&wheel_handler[0]);
    if(sta!=0){
        println("wheel0 left motor init fail! xp_pans_init() return code=%d",sta);
        return -1;
    }
    if(xp_pans_round_block(&wheel_handler[0],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[0],block_round+1,7,6,524288)!=0)return -3;
    if(xp_pans_update_block(&wheel_handler[0],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[0],5,block_event)!=0)return -3;
    if(xp_pans_stop_block(&wheel_handler[0],6,0)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[0],7,7,block_round,524296)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[0],8,7)!=0)return -3;
    if(xp_pans_set_block(&wheel_handler[0],6)!=0)return -4;
    xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
    if(xp_pans_run(&wheel_handler[0],pans_on)!=0)return -5;
#if MOTOR_num==2
    if((sta=xp_pans_init(&wheel_handler[1]))!=0){
        println("wheel1 right motor init fail! xp_pans_init() return code=%d",sta);
        return -2;
    }
    if(xp_pans_round_block(&wheel_handler[1],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[1],block_round+1,7,6,524288)!=0)return -3;
    if(xp_pans_update_block(&wheel_handler[1],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[1],5,block_event)!=0)return -3;
    if(xp_pans_stop_block(&wheel_handler[1],6,0)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[1],7,7,block_round,524296)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[1],8,7)!=0)return -3;
    if(xp_pans_set_block(&wheel_handler[1],6)!=0)return -4;
    xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
    if(xp_pans_run(&wheel_handler[1],pans_on)!=0)return -5;
#endif
    
    println("agv wheel recover success~");
    agv_wheel.init=1;
    return 0;
}

/*
wheel motor init
return:
    0=success
    -1=fail
*/
int xp_wheel_init(u8 port,int speed,u8 slave_id){
    int sta=0;

    if(agv_wheel.init==1){
        return 0;
    }
    xp_wheel_config();
    wheel_handler[0].port=port;
    wheel_handler[0].speed=speed;
    // wheel_handler[0].slave_id=slave_id;
    wheel_handler[1].port=port;
    wheel_handler[1].speed=speed;
    // wheel_handler[1].slave_id=slave_id+1;
    agv_wheel.motor[0]=&wheel_handler[0];
    agv_wheel.motor[1]=&wheel_handler[1];
    agv_wheel.turn=wheel_middle;
	
    sta=xp_pans_init(&wheel_handler[0]);
    if(sta!=0){
        println("wheel0 left motor init fail! xp_pans_init() return code=%d",sta);
        return -1;
    }
    if(xp_pans_round_block(&wheel_handler[0],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[0],block_round+1,7,6,524288)!=0)return -3;
    if(xp_pans_update_block(&wheel_handler[0],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[0],5,block_event)!=0)return -3;
    if(xp_pans_stop_block(&wheel_handler[0],6,0)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[0],7,7,block_round,524296)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[0],8,7)!=0)return -3;
    if(xp_pans_set_block(&wheel_handler[0],6)!=0)return -4;
    xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
    if(xp_pans_run(&wheel_handler[0],pans_on)!=0)return -5;
#if MOTOR_num==2
    if((sta=xp_pans_init(&wheel_handler[1]))!=0){
        println("wheel1 right motor init fail! xp_pans_init() return code=%d",sta);
        return -2;
    }
    if(xp_pans_round_block(&wheel_handler[1],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[1],block_round+1,7,6,524288)!=0)return -3;
    if(xp_pans_update_block(&wheel_handler[1],1,WHEEL_left_fw)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[1],5,block_event)!=0)return -3;
    if(xp_pans_stop_block(&wheel_handler[1],6,0)!=0)return -3;
    if(xp_pans_compare_block(&wheel_handler[1],7,7,block_round,524296)!=0)return -3;
    if(xp_pans_jump_block(&wheel_handler[1],8,7)!=0)return -3;
    if(xp_pans_set_block(&wheel_handler[1],6)!=0)return -4;
    xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
    if(xp_pans_run(&wheel_handler[1],pans_on)!=0)return -5;
#endif
    // sta=xp_pans_init_block(&wheel_handler[0]);
    // if(sta!=0){
    //     println("wheel left motor init block fail!");
    //     return -3;
    // }
    // if(xp_pans_init_block(&wheel_handler[1])!=0){
    //     println("wheel right motor init block fail!");
    //     return -4;
    // }
    aos_mutex_new(&wheel_mux);
    println("agv wheel init success~");
    aos_task_new("speed check",xp_wheel_run_check_thread,NULL,3100);
    agv_wheel.init=1;
    return 0;
}

/*clear wheel warn
*/
int xp_clear_wheel_warn(void){
    u8 clear_warn_try_again_cnt;

    //检测是否清成功，不成功重试
    clear_warn_try_again_cnt = 5;
    while(clear_warn_try_again_cnt--){
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);

        if(xp_pans_get_warn_state(&wheel_handler[0])==pans_on){
            aos_msleep(50);
            if(xp_pans_en(&wheel_handler[0],pans_off)!=0){
                println("wheel 0 enable off faild");
                continue;
            }
            aos_msleep(50);
            if(xp_pans_clear_warn(&wheel_handler[0])!=0){
                println("wheel 0 warn clear faild");
                continue;
            }
            aos_msleep(50);
            if(xp_pans_en(&wheel_handler[0],pans_on)!=0){
                println("wheel 0 enable config faild");
                continue;
            }
        }
#if MOTOR_num==2
        if(xp_pans_get_warn_state(&wheel_handler[1])==pans_on){
            aos_msleep(50);
            if(xp_pans_en(&wheel_handler[1],pans_off)!=0){
                println("wheel 1 enable off faild");
                continue;
            }
            aos_msleep(50);
            if(xp_pans_clear_warn(&wheel_handler[1])!=0){
                println("wheel 1 warn clear faild");
                continue;
            }
            aos_msleep(50);
            if(xp_pans_en(&wheel_handler[1],pans_on)!=0){
                println("wheel 1 enable config faild");
                continue;
            }
        }
#endif
        println("wheel warn clear success! try cnt = %d",(5-clear_warn_try_again_cnt));
        return 0;
    }

    println("wheel warn clear faild!!!");
    return -1;
}

/*
check motor error state
*/
int xp_wheel_check_error(void){
    u16 err_code = 0;

    for(u8 i=0;i<2;i++){
        if(xp_pans_get_warn_state(&wheel_handler[i])==pans_on){
            println("wheel %d have a error!",wheel_handler[i].slave_id);
            err_code = xp_pans_get_error_code(&wheel_handler[i]);
            if((err_code>>8) == 13){
                //电压不足异常暂时屏蔽
                println("wheel %d low_voltage", wheel_handler[i].slave_id);
                continue;
            }
            return -1;
            // if(xp_pans_clear_warn(&wheel_handler[i])!=0)return -1;
            // if(xp_pans_en(&wheel_handler[i],pans_on)!=0)return -2;
        }
    }
    return 0;
}

/*
wheel motor run
param:
    run:run state,1=启动，0=停止
    dir:run direction，0=正转，1=反转
return:
    0=success
    -1=fail    
*/
int xp_motor_run(u8 run,u8 dir){
    int sta=0,rec1=0,rec2=0,rec3=0,rec4=0;
    if(xp_pans_get_warn_state(&wheel_handler[0])==pans_on){
        if(xp_pans_clear_warn(&wheel_handler[0])!=0)return -13;
        if(xp_pans_en(&wheel_handler[0],pans_on)!=0)return -14;
    }
    #if MOTOR_num==2
    if(xp_pans_get_warn_state(&wheel_handler[1])==pans_on){
        if(xp_pans_clear_warn(&wheel_handler[1])!=0)return -15;
        if(xp_pans_en(&wheel_handler[1],pans_on)!=0)return -16;
    }
    #endif
    println("motor run,dir=%d",dir);
    if(run==1){
        if(dir==0){
            agv_wheel.dir=1;
        }
        else{
            agv_wheel.dir=-1;
        }
        if(xp_pans_slow_stop(&wheel_handler[0],pans_off)!=0)return -11;
        if(xp_pans_run(&wheel_handler[0],pans_off)<0)return -2;
        if(xp_pans_confg_round(&wheel_handler[0],0,dir)!=0)return -3;
        #if MOTOR_num==2
        if(xp_pans_slow_stop(&wheel_handler[1],pans_off)!=0)return -12;
        if(xp_pans_run(&wheel_handler[1],pans_off)<0)return -5;
        if(xp_pans_confg_round(&wheel_handler[1],0,dir)!=0)return -6;
        #endif
        // if(xp_io_read_input_pin(1,4)!=0){
        //     println("error:pansonic motor0 error!");
        //     return -10;
        // }
        // #if MOTOR_num==2
        // if(xp_io_read_input_pin(1,5)!=0){
        //     println("error:pansonic motor1 error!");
        //     return -10;
        // }
        // #endif
        rec1=xp_pans_run(&wheel_handler[0],pans_on);
#if MOTOR_num==2
        rec2=xp_pans_run(&wheel_handler[1],pans_on);
#endif
        if(rec1!=0||rec2!=0){
            aos_reboot();
            return -8;
        }
    }
    else if(run==0){
        agv_wheel.dir=0;
        rec3=xp_pans_slow_stop(&wheel_handler[0],pans_on);
        #if MOTOR_num==2
        rec4=xp_pans_slow_stop(&wheel_handler[1],pans_on);
        #endif
        if(rec3!=0||rec4!=0){
            aos_reboot();
            return -10;
        }
        aos_msleep(500);
    }
    else{
        println("warn:xp_motor_run() run input error,run=%d",run);
        return -3;
    }
    return 0;
}

/*
wheels move
param:
    speed:速度设置
    dir:后退<0,停止=0,前进>0
return:
    0=success
*/
int xp_wheel_move(int speed,s8 dir){
    int sta=0;
    u8 diirect[2];
    int wait_time=0;

    wait_time=500*wheel_handler[0].config.speed[agv_wheel.speed_block[0]]/3000;
    if(dir<0){
        if(agv_wheel.dir>0){
            aos_msleep(100);
        }
        diirect[0]=WHEEL_left_bk;
        diirect[1]=WHEEL_right_bk;
        println("wheel move bk,speed=%d",speed);
    }
    else if(dir>0){
        if(agv_wheel.dir<0){
            aos_msleep(100);
        }
        diirect[0]=WHEEL_left_fw;
        diirect[1]=WHEEL_right_fw;
        println("wheel move fw,speed=%d",speed);
    }
    else{
        println("wheel stop!");
		agv_wheel.dir=0;
        agv_wheel.speed[0]=agv_wheel.speed[1]=0;
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
        aos_msleep(wait_time+100);
        if(xp_pans_en(&wheel_handler[0],pans_off)!=0)return -3;
        if(xp_pans_en(&wheel_handler[1],pans_off)!=0)return -3;
        aos_msleep(500);
        if(xp_pans_en(&wheel_handler[0],pans_on)!=0)return -3;
        if(xp_pans_en(&wheel_handler[1],pans_on)!=0)return -3;
        // xp_pans_slow_stop(&wheel_handler[0],pans_off);
        // xp_pans_slow_stop(&wheel_handler[1],pans_off);
    }
    if(dir!=0){
        if(agv_wheel.dir!=dir&&agv_wheel.dir!=0&&dir!=0){
            println("wheel stop!");
            xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
            xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
            aos_msleep(wait_time);
        }
        agv_wheel.speed[0]=agv_wheel.speed[1]=speed;
        if(xp_pans_change_speed(&wheel_handler[0],1,speed)!=0)return -2;
        if(xp_pans_change_speed(&wheel_handler[1],1,speed)!=0)return -3;
        if(xp_pans_update_block(&wheel_handler[0],1,diirect[0])!=0)return -4;
        if(xp_pans_update_block(&wheel_handler[1],1,diirect[1])!=0)return -5;
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_run);
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_run);
    }
    agv_wheel.dir=dir;
    agv_wheel.turn=wheel_middle;
    return 0;
}


/*
wheel turn
param:
    dir:turn direction left<0,middle=0,right>0
return:
    0=success
*/
int xp_wheel_turn(WHEEL_turn dir,int s){
    u8 speed_block=0;
    WHEEL_turn turn;
    u8 *agv_speed,foot=0;
    u8 direct;
    int wait_time;

    switch(dir){
        case wheel_left:
            // speed_block=agv_wheel.speed_block[0]-1;
            // agv_speed=&agv_wheel.speed_block[0];
            foot=0;
            turn=wheel_left;
            if(agv_wheel.dir>0){
                direct=WHEEL_left_fw;
            }
            else if(agv_wheel.dir==0){
                foot=1;
                direct=WHEEL_right_bk;
            }
            else if(agv_wheel.dir<0){
                foot=1;
                direct=WHEEL_right_bk;
            }
            println("wheel turn to left");
            break;
        case wheel_middle:
            if(agv_wheel.turn==wheel_left){
                // speed_block=agv_wheel.speed_block[1];
                // agv_speed=&agv_wheel.speed_block[0];
                foot=0;
                if(agv_wheel.dir>0){
                    direct=WHEEL_left_fw;
                }
                else if(agv_wheel.dir==0){
                    foot=1;
                    direct=WHEEL_right_fw;
                }
                else if(agv_wheel.dir<0){
                    foot=1;
                    direct=WHEEL_right_bk;
                }
            }
            else if(agv_wheel.turn==wheel_right){
                // speed_block=agv_wheel.speed_block[0];
                // agv_speed=&agv_wheel.speed_block[1];
                foot=1;
                if(agv_wheel.dir>0){
                    direct=WHEEL_right_fw;
                }
                else if(agv_wheel.dir==0){
                    foot=0;
                    direct=WHEEL_left_fw;
                }
                else if(agv_wheel.dir<0){
                    foot=0;
                    direct=WHEEL_left_bk;
                }
            }
            else{
                return 0;
            }
            turn=wheel_middle;
            println("wheel turn to middle");
            break;
        case wheel_right:
            // speed_block=agv_wheel.speed_block[0]-1;
            // agv_speed=&agv_wheel.speed_block[1];
            foot=1;
            if(agv_wheel.dir>0){
                direct=WHEEL_right_fw;
            }
            else if(agv_wheel.dir==0){
                foot=0;
                direct=WHEEL_left_bk;
            }
            else if(agv_wheel.dir<0){
                foot=0;
                direct=WHEEL_left_bk;
            }
            turn=wheel_right;
            println("wheel turn to right");
            break;
        default:
            println("error:xp_wheel_turn() input turn direction error,rurn=%d",turn);
            return -1;;
    }
    if(xp_wheel_check_error()!=0)return -3;
    if(agv_wheel.dir!=0&&turn!=wheel_middle){
        agv_wheel.speed[foot]=agv_wheel.normal_speed-s;
        if(xp_pans_change_speed(&wheel_handler[foot],1,agv_wheel.speed[foot])!=0)return -4;
        // if(xp_pans_confg_updateSpeed(&wheel_handler[foot],0,wheel_handler[0].config.speed[1]-s,direct)!=0)return -6;
    }
    else if(agv_wheel.dir==0&&turn==wheel_middle){
        agv_wheel.speed[foot]=0;
        xp_agv_io_set(agv_wheel.io[foot],WHEEL_io_stop);
        aos_msleep(500);
    }
    else{
        if(agv_wheel.dir!=0){
            agv_wheel.speed[foot]=agv_wheel.normal_speed;
            if(xp_pans_change_speed(&wheel_handler[foot],1,agv_wheel.speed[foot])!=0)return -5;
            // if(xp_pans_confg_updateSpeed(&wheel_handler[foot],0,wheel_handler[0].config.speed[1],direct)!=0)return -6;
        }
        else{
            agv_wheel.speed[foot]=agv_wheel.normal_speed-s;
            if(xp_pans_change_speed(&wheel_handler[foot],1,agv_wheel.speed[foot])!=0)return -6;
            if(xp_pans_update_block(&wheel_handler[foot],1,direct)!=0)return -7;
            xp_agv_io_set(agv_wheel.io[foot],WHEEL_io_run);
        }
//        if(agv_wheel.dir==0&&agv_wheel.turn!=wheel_middle){
//            aos_msleep(wait_time*10);
//            xp_pans_slow_stop(&wheel_handler[foot],pans_on);
//            xp_pans_slow_stop(&wheel_handler[foot],pans_off);
//        }
    }
    // *agv_speed=speed_block;
    agv_wheel.turn=turn;
    return 0;
}


/*
wheel round a angle
param: 
    angle:round angle
return:
    0=success
*/
int xp_wheel_round(WHEEL_turn turn,float angle){
    u8 dir[2];
    int sta=0;
    float imu[2];

    if(agv_wheel.dir!=0){
        println("error:please stop wheel motor first!");
        return -1;
    }
    if(turn==wheel_left){
        dir[0]=WHEEL_left_bk;
        dir[1]=WHEEL_right_fw;
        println("wheel round to left ! angle=%02f",angle);
    }
    else if(turn==wheel_right){
        dir[0]=WHEEL_left_fw;
        dir[1]=WHEEL_right_bk;
        println("wheel round to right ! angle=%02f",angle);
    }
    if(angle==0){
        agv_wheel.speed[0]=agv_wheel.speed[1]=0;
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
        aos_msleep(500);
        return 0;
    }
    if(xp_wheel_check_error()!=0)return -11;
    agv_wheel.speed[0]=agv_wheel.speed[1]=agv_wheel.normal_speed;
    if(xp_pans_change_speed(&wheel_handler[0],1,agv_wheel.speed[0])!=0)return -2;
    if(xp_pans_change_speed(&wheel_handler[1],1,agv_wheel.speed[1])!=0)return -3;
    if(xp_pans_update_block(&wheel_handler[0],1,dir[0])!=0)return -4;
    if(xp_pans_update_block(&wheel_handler[1],1,dir[1])!=0)return -5;
    xp_agv_io_set(agv_wheel.io[0],WHEEL_io_run);
    xp_agv_io_set(agv_wheel.io[1],WHEEL_io_run);
    return 0;
}

int xp_wheel_set_wheel_locker(int val)
{
	wheel_locker = val;
}

int xp_wheel_get_wheel_locker()
{
	return wheel_locker;
}

/*
set wheel motors speed
单独设置两个电机速度，参数单位是电机的速度单位，r/min,正负表示正反
param:
    speed0:left wheel motor speed
    speed1:right wheel motor speed
return:
    0=success
*/
int xp_wheel_motorSpeed_set(int speed0,int speed1){
    u8 flag0=0,flag1=0;
#define LINE_VEL2PLUS    (21.0 * 60 / 3.1415926 / 0.184)
	if(1 == wheel_locker){
		if(0 != speed0 || 0 != speed1){
			return 0;
		}
	}
    //蓝牙遥控接口会直接调用此函数,因此这里也需要获取命令速度
    wheel_cmd_speed[0] = speed0 / LINE_VEL2PLUS;
    wheel_cmd_speed[1] = speed1 / LINE_VEL2PLUS;

    if(agv_wheel.init!=1)return -1;
    if(aos_mutex_lock(&wheel_mux,500)!=0){
        perr("wheel set speed fail,wait sem time out !");
        return -12;
    }
    agv_wheel.speeds[0]=speed0;
    agv_wheel.speeds[1]=speed1;
    if(speed0<0){
        agv_wheel.speed[0]=0-speed0;
        agv_wheel.dirs[0]=WHEEL_left_bk;
    }
    else if(speed0>0){
        agv_wheel.speed[0]=speed0;
        agv_wheel.dirs[0]=WHEEL_left_fw;
    }
    if(speed1<0){
        agv_wheel.speed[1]=0-speed1;
        agv_wheel.dirs[1]=WHEEL_right_bk;
    }
    else if(speed1>0){
        agv_wheel.speed[1]=speed1;
        agv_wheel.dirs[1]=WHEEL_right_fw;
    }

    //println("wheel motor speed: left=%d     right=%d",speed0,speed1);
#ifdef WASHER_MERGE
    if(xp_wheel_error_io_check()!=0){
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
        xp_wheel_check_error();
        aos_mutex_unlock(&wheel_mux);
        return -10;
    }
#else
    if(xp_wheel_check_error()!=0){
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
        aos_mutex_unlock(&wheel_mux);
        return -11;
    }
#endif
	//如果两个轮子的速度都为0，则需要同步停止，避免一先一后导致扭动
	if(speed0==0 && speed1==0){
		u32 states=0;
		agv_wheel.speed[0]=0;
		agv_wheel.speed[1]=0;
		states=(((u8)WHEEL_io_stop)<<4)+(((u8)WHEEL_io_stop)<<5);
		xp_io_write_pins(1,states,4,2);
		//println("Dual wheels sync stop!");
        aos_mutex_unlock(&wheel_mux);
		return 0;
	}
	
    if(speed0==0){
        agv_wheel.speed[0]=0;
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
    }
    else{
        if(xp_pans_change_speed(&wheel_handler[0],1,agv_wheel.speed[0])!=0){
						xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
						xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
						println("wheel 0 set speed fail~");
                        aos_mutex_unlock(&wheel_mux);
						return -2;
				}
        if(xp_pans_update_block(&wheel_handler[0],1,agv_wheel.dirs[0])!=0){
						xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
						xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
						println("wheel 0 update block fail~");
                        aos_mutex_unlock(&wheel_mux);
						return -4;
				}
        flag0=1;
    }
    if(speed1==0){
        agv_wheel.speed[1]=0;
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
    }
    else{
        if(xp_pans_change_speed(&wheel_handler[1],1,agv_wheel.speed[1])!=0){
						xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
						xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
						println("wheel 1 set speed fail~");
                        aos_mutex_unlock(&wheel_mux);
						return -3;
				}
        if(xp_pans_update_block(&wheel_handler[1],1,agv_wheel.dirs[1])!=0){
						xp_agv_io_set(agv_wheel.io[0],WHEEL_io_stop);
						xp_agv_io_set(agv_wheel.io[1],WHEEL_io_stop);
						println("wheel 1 update block fail~");
                        aos_mutex_unlock(&wheel_mux);
						return -4;
				}
        flag1=1;
    }

    if(flag0==1){
        xp_agv_io_set(agv_wheel.io[0],WHEEL_io_run);
    }
    if(flag1==1){
        xp_agv_io_set(agv_wheel.io[1],WHEEL_io_run);
    }
    // xp_wheel_run_check_notify();
    aos_mutex_unlock(&wheel_mux);
    return 0;
}


/*
set motor line speed
param:
    line_speed0:left wheel motor line speed
    line_speed1:right wheel .....
return:
    0=success
*/
int xp_wheel_set_wheel_speed(float line_speed0,float line_speed1){
    int s[2]={0};
    int ret = 0;
    u8 retry_cnt = 5;

	wheel_cmd_speed[0] = line_speed0;
    wheel_cmd_speed[1] = line_speed1;

    s[0] = xp_wheel_get_speed(line_speed0);
    s[1] = xp_wheel_get_speed(line_speed1);

    if(agv_wheel.locker != wheel_locker_release){
        println("agv wheel locked as: %s .",xp_lockerString_get(agv_wheel.locker));
        return agv_wheel.locker;
    }
    
    ret = xp_wheel_motorSpeed_set(s[0],s[1]);
    while(ret != 0 && retry_cnt > 0){
        retry_cnt--;
        println("wheel speed set fail,ret = %d,retry_cnt = %d",ret,retry_cnt);
        xp_wheel_motorSpeed_set(s[0],s[1]);
        aos_msleep(300);
    }
    
    return ret;
}


/*
set wheels stop
return:
    0=success
*/
int xp_wheel_stop(void){
    println("agv wheels set stop !");
	wheel_cmd_speed[0] = 0;
    wheel_cmd_speed[1] = 0;
    return xp_wheel_motorSpeed_set(0,0);
}

/*
set motor acc
param:
    acc:acc value, 0=not set,时间值 值越小，加速度越大
    dec:dec value, 0=not set,时间值 值越小，减速度越大
    block:block num
return:
    0=success
*/
int xp_wheel_set_acc(u16 acc,u16 dec,u8 block){
    if(xp_pans_set_acc(&wheel_handler[0],acc,dec,block)!=0)return -1;
    if(xp_pans_set_acc(&wheel_handler[1],acc,dec,block)!=0)return -2;
    return 0;
}


/*
wheel get motor speed
param:
    line_speed:wheeel line speed m/s
return:motor speed
*/
int xp_wheel_get_speed(float line_speed){
    float wheel_r;
    float motor=0;

    if(line_speed<0.0001&&line_speed>-0.0001)return 0;
    wheel_r=line_speed/(3.141593*0.184);
    motor=wheel_r*21.0*60.0;
    return (int)motor;
}

/*
get wheel motor dir state
*/
int xp_wheel_get_dir(void){
    return agv_wheel.dir;
}

WHEEL_object *xp_wheel_object_get(void){
    return &agv_wheel;
}

/*************************************agv test***************************************/
static u8 agv_test=0,agv_turn=3;

void xp_agv_wheel_test(void){
    int sta=0;

    if(agv_test==0)return;
    if(wheel_handler[0].init==1&&wheel_handler[1].init!=1)return;
    if(xp_wheel_move(agv_wheel.normal_speed,-1)!=0){agv_test=0;goto END;}
    aos_msleep(1000);
    if(xp_wheel_move(agv_wheel.normal_speed,0)!=0){agv_test=0;goto END;}
    aos_msleep(1000);
    if(xp_wheel_move(agv_wheel.normal_speed,1)!=0){agv_test=0;goto END;}
    aos_msleep(1000);
END:xp_wheel_move(agv_wheel.normal_speed,0);
    aos_msleep(1000);
}


/********************************wheel motor test*************************************/
static u8 test=0,back=0,fw=0,get_speed=0,get_encode=0;
static u16 run_time=500,set_speed=0,count_max=10000;
/*
wheel test
*/
void xp_wheel_test(void){
    xp_wheel_cycle();
    xp_motor_back(run_time);
    xp_motor_fw(run_time);
    if(set_speed>0){
        if(xp_pans_change_speed(&wheel_handler[0],0,set_speed)!=0){
            println("pamsonic motor0 set speed error!");
        }
        #if MOTOR_num==2
        if(xp_pans_change_speed(&wheel_handler[1],0,set_speed)!=0){
            println("pamsonic motor1 set speed error!");
        }
        #endif
        set_speed=0;
    }
}

/*
wheel cycle run
*/
void xp_wheel_cycle(void){
    int sta=0;
    static u16 count=1;

    if(test==0){count=0;return;}
    if(wheel_handler[0].init==0){test=0;return;}
    println("%d wheel motor run positive!",count);
    sta=xp_motor_run(1,0);
    if(sta!=0){test=0;return;}
    aos_msleep(run_time);
    println("%d wheel motor stop!",count);
    sta=xp_motor_run(0,0);
    if(sta!=0){test=0;return;}
    aos_msleep(1000);
    println("%d wheel motor run reversal!",count);
    sta=xp_motor_run(1,1);
    if(sta!=0){test=0;return;}
    aos_msleep(run_time);
    println("%d wheel motor stop!",count);
    sta=xp_motor_run(0,0);
    if(sta!=0){test=0;return;}
    count++;
    if(count>=count_max)test=0;
    aos_msleep(1000);
}


/*
wheel motor read actual speed
*/
void xp_wheel_read_speed(void){
    int speed_left = 0,speed_right = 0;

    if(test!=1)return;
    speed_left  = 0-xp_pans_get_actual_speed(&wheel_handler[0]);
    speed_right = xp_pans_get_actual_speed(&wheel_handler[1]);
    println("motor actual speed: left_wheel=%d, right_wheel=%d",speed_left,speed_right);
}


/*
wheel motor read encode
*/
void xp_wheel_read_encode(void){
    PANS_encode encode1,encode2;
    if(get_encode==0)return;
    xp_pans_get_encode(&wheel_handler[0],&encode1);
    xp_pans_get_encode(&wheel_handler[1],&encode2);
    println("motor encode:  %d.%d   %d.%d",encode1.r,encode1.p,encode2.r,encode2.p);
}

/*
wheel motor get encode 
*/
int xp_wheel_get_encode(uint64_t *encodeBuf)
{
    if (NULL != encodeBuf){
		PANS_encode encode1,encode2;
		if (0 != xp_pans_get_encode(&wheel_handler[0],&encode1)) {return -1;}
		if (0 != xp_pans_get_encode(&wheel_handler[1],&encode2)) {return -1;}
		encodeBuf[0] = encode1.h;
		encodeBuf[1] = encode2.h;
		return 0;
	}
	return -2;
}

/*
motor back
*/
void xp_motor_back(u16 time){
    if(back==0)return;
    back=0;
    xp_motor_run(1,1);
    aos_msleep(time);
    xp_motor_run(0,0);
}

void xp_motor_fw(u16 time){
    if(fw==0)return;
    fw=0;
    xp_motor_run(1,0);
    aos_msleep(time);
    xp_motor_run(0,0);
}

int xp_wheel_enable_set(bool val)
{
	if(1 == val){
		if(xp_pans_en(&wheel_handler[0],pans_on)!=0)return -3;
		if(xp_pans_en(&wheel_handler[1],pans_on)!=0)return -3;
	}else{
		if(xp_pans_en(&wheel_handler[0],pans_off)!=0)return -3;
		if(xp_pans_en(&wheel_handler[1],pans_off)!=0)return -3;
	}
	
	return 0;
}

/*
get speed different from turn radius and base speed
从拐弯半径和基准速度获取速差
param:
    rad:turn radius :m
    speed: base speed :m/s
return:speed different value  速差 m/s
*/
float xp_wheel_diffSpeed_get(float rad,float speed){
    float ov;
    ov=AGV_R*speed/rad;
    return ov;
}

/*
* get current wheel's distance
* 获取当前左右轮的里程数据
* param[in]:
* param[out]: wheel_l，左轮里程数据，单位mm
              wheel_r, 右轮里程数据，单位mm
*/
void xp_wheel_encode_get(uint64_t *encode_l, uint64_t *encode_r)
{
	PANS_encode encode[2];
	
    xp_pans_get_encode(&wheel_handler[0],&encode[0]);
    xp_pans_get_encode(&wheel_handler[1],&encode[1]);

	*encode_l = encode[0].h;
	*encode_r = encode[1].h;
}


/*
check wheel driver error io state
return:
    0=normal
    -1=left motor error
    -2=right motor error
    -3=all motor error
*/
int xp_wheel_error_io_check(void){
    int left=0,right=0;
    int sta=0;

    left=xp_agv_io_read(WHEEL_io_left_error);
    right=xp_agv_io_read(WHEEL_io_left_error);
    if(left==1){
        sta &=0x01;
    }
    if(right==1){
        sta &=0x02;
    }
    if(sta!=0){
        perr("wheel driver error ,left=%d,right=%d",left,right);
    }
    return 0-sta;
}


/*
get enum type string
return: string
*/
const char *xp_lockerString_get(WHEEL_locker_type type){
    switch (type)
    {
    case wheel_locker_release:
        get_param_string(wheel_locker_release);
        break;
    case wheel_locker_safe:
        get_param_string(wheel_locker_safe);
        break;
    case wheel_locker_near:
        get_param_string(wheel_locker_near);
        break;
    case wheel_locker_wait:
        get_param_string(wheel_locker_wait);
        break;
    case wheel_locker_rang_max:
        get_param_string(wheel_locker_rang_max);
        break;
    case wheel_locker_rang_min:
        get_param_string(wheel_locker_rang_min);
        break;
    default:
        perr("type value fault !,out of range, type= %d.",type);
        return "out of range value.";
    }
}


/*
wheel locker set
param:
    type: locker type
return:
    0=success
    -1= type value fail
    -2= wheel locker stop wheels fail
*/
int xp_wheel_locker_set(WHEEL_locker_type type){
    int count =0;

    if(type<wheel_locker_release||type>=wheel_locker_max){
        perr("wheel locker set fail, out of range. type= %d.",type);
        return -1;
    }
    println("wheel locker set as: %s.",xp_lockerString_get(type));
    agv_wheel.locker=type;
    if(type != wheel_locker_release){
        while(xp_wheel_stop()!=0){
            aos_msleep(500);
            count++;
            if(count>10){
                perr("locker wheel  and stop wheels fail !");
                return -2;
            }
        }
        
    }

    return 0;
}

/*
wheel locker get
return:type
*/
WHEEL_locker_type xp_wheel_locker_get(void){
    return agv_wheel.locker;
}
//获取实时轮速,agv在移动状态返回true
bool xp_wheel_get_wheel_speed(float * const speed_L, float * const speed_R)
{
	*speed_L = wheel_cmd_speed[0];
	*speed_R = wheel_cmd_speed[1];
	
	if ((int)(*speed_L * 100) || (int)(*speed_R * 100)){
		return true;
	}else{
		return false;
	}
}

/*
轮子不动作测试功能，在轮子启动后获取实际速度，如果速度仍为0 ，则读取i/f STB 状态和 监控STB状态
使用：
    在启动轮子的地方后面加上设定通知即可 xp_wheel_run_check_notify()
*/
// static aos_mutex_t wheel_runCheck_mux;

static void xp_wheel_run_check_thread(void *arg){
    int speed[2]={0};
    PANS_type stb120=0,stb130=0;
    // aos_mutex_new(&wheel_runCheck_mux);

    while(1){
        // aos_mutex_lock(&wheel_runCheck_mux,AOS_WAIT_FOREVER);
        aos_msleep(1000);
        speed[0]=xp_pans_get_actual_speed(agv_wheel.motor[0]);
        speed[1]=xp_pans_get_actual_speed(agv_wheel.motor[1]);
        println("wheel speed: set %d    %d; real:%d     %d;  r/min.",
                agv_wheel.speed[0],agv_wheel.speed[1],speed[0],speed[1]);
        if(abs(agv_wheel.speed[0])>20&&abs(speed[0])<10){
            stb120=xp_pans_get_stb120_state(agv_wheel.motor[0]);
            stb130=xp_pans_get_run_state(agv_wheel.motor[0]);
            perr("wheel left speed abnormal:stb120: %d, stb130: %d.",stb120,stb130);
        }
        if(abs(agv_wheel.speed[1])>20&&abs(speed[1])<10){
            stb120=xp_pans_get_stb120_state(agv_wheel.motor[1]);
            stb130=xp_pans_get_run_state(agv_wheel.motor[1]);
            perr("wheel right speed abnormal:stb120: %d, stb130: %d.",stb120,stb130);
        }
        stb120=pans_off;
        stb130=pans_off;

    }
}

/*
通知检测轮速
*/
void xp_wheel_run_check_notify(void){
//    aos_mutex_unlock(&wheel_runCheck_mux);
}

/*****************************************debug**************************************/
/*
wheel debug
*/
int xp_wheel_debug(char *type,char *fun,char *param){
    static float set_run_speed=0.15;
    float f;

    if(type==NULL)return 0;
    if(strcmp(type,"wheel")!=0)return 0;
    if(fun==NULL)return 0;
    if(strcmp(fun,"test")==0){
        if(strcmp(param,"start")==0){
            println("wheel test start!");
            test=1;
        }
        else if(strcmp(param,"stop")==0){
            println("wheel test stop!");
            test=0;
        }
        else{
            return 0;
        }
    }
    else if(strcmp(fun,"clear")==0){
        println("wheel clear error!");
        xp_pans_clear_warn(&wheel_handler[0]);
    }
    else if(strcmp(fun,"motor_stop")==0){
        xp_pans_run(&wheel_handler[0],pans_off);
    }
    else if(strcmp(fun,"en")==0){
        if(strcmp(param,"1")==0){
            xp_pans_en(&wheel_handler[0],pans_on);
        }
        else if(strcmp(param,"0")==0){
            xp_pans_en(&wheel_handler[0],pans_off);
        }
        else if(strcmp(param,"read")==0){
            if(xp_pans_get_en_state(&wheel_handler[0])){
                println("en success");
            }
            else{
                println("en fail~");
            }

        }
        else if(strcmp(param,"data")==0){
            xp_pans_get_en_data(&wheel_handler[0]);
        }
    }
    else if(strcmp(fun,"run")==0){
        f=atof(param);
        if(f>0.0001||f<-0.0001){
            set_run_speed=f;
        }
        xp_wheel_set_wheel_speed(f,f);
    }
    else if(strcmp(fun,"error_io")==0){
        xp_wheel_error_io_check();
    }
    else if(strcmp(fun,"back")==0){
        println("wheel motor back run %dms",run_time);
        back=1;
    }
    else if(strcmp(fun,"fw")==0){
        println("wheel motor fw run %dms",run_time);
        fw=1;
    }
    else if(strcmp(fun,"run_time")==0){
        run_time=atoi(param);
        println("wheel test run time set as:%d",run_time);
    }
    else if(strcmp(fun,"get")==0){
        if(strcmp(param,"run_time")==0){
            println("wheel run time=%d",run_time);
        }
        else{
            return 0;
        }

    }
    else if(strcmp(fun,"set_speed")==0){
        u16 speed=atoi(param);
        if(speed>0||speed<6000){
            set_speed=speed;
            println("wheel motor speed set as:%d",speed);
        }
        else{
            println("wheel motor speed set error,%d",speed);
        }
    }
    else if(strcmp(fun,"encode")==0){
        if(strcmp(param,"1")==0){
            get_encode=1;
        }
        else if(strcmp(param,"0")==0){
            get_encode=0;
        }
        else{
            return 0;
        }
    }
    else if(strcmp(fun, "move")==0){
        if(strcmp(param,"fw")==0){
            xp_wheel_move(agv_wheel.normal_speed,1);
        }
        else if(strcmp(param,"bk")==0){
            xp_wheel_move(agv_wheel.normal_speed,-1);
        }
        else if(strcmp(param,"stop")==0){
            xp_wheel_move(agv_wheel.normal_speed
            ,0);
        }
        else if(strcmp(param,"test")==0){
            agv_test=1;
        }
        else if(strcmp(param,"test_stop")==0){
            agv_test=0;
        }
    }
    else if(strcmp(fun,"round")==0){
        if(strcmp(param,"left")==0){
            xp_wheel_set_wheel_speed(0.0-set_run_speed,set_run_speed);
        }
        else if(strcmp(param,"right")==0){
            xp_wheel_set_wheel_speed(set_run_speed,0.0-set_run_speed);
        }
    }
    else if(strcmp(fun,"turn")==0){
        if(strcmp(param,"left")==0){
            xp_wheel_set_wheel_speed(set_run_speed/2,set_run_speed);
        }
        else if(strcmp(param,"right")==0){
            xp_wheel_set_wheel_speed(set_run_speed,set_run_speed/2);
        }
    }
    else if(strcmp(fun,"speed_block")==0){
        u8 block=0;
        block=atoi(param);
        if(block<5){
            agv_wheel.speed_block[0]=agv_wheel.speed_block[1]=block;
        }
        else{
            println("error:speed block set fail,set block number=%d",block);
        }
    }
    else if(strcmp(fun,"count")==0){
        count_max=atoi(param);
    }
    else if(strcmp(fun,"speed")==0){
        double line_speed=atof(param);
        u16 speed=xp_wheel_get_speed(line_speed);
        xp_pans_change_speed(&wheel_handler[0],1,speed);
        // xp_agv_io_set(1,0);
    }
    else if(strcmp(fun,"test_run")==0){
        if(xp_pans_slow_stop(&wheel_handler[0],pans_off)!=0)return -11;
        if(xp_pans_run(&wheel_handler[0],pans_off)<0)return -2;
        if(xp_pans_confg_round(&wheel_handler[0],0,0)!=0)return -3;
        xp_pans_run(&wheel_handler[0],pans_on);
    }
    else if(strcmp(fun,"test_stop")==0){
        xp_pans_slow_stop(&wheel_handler[0],pans_on);
    }
    else if(strcmp(fun,"set_motors")==0){
        if(strcmp(param,"left")==0){
            xp_wheel_motorSpeed_set(100,200);
        }
        else if(strcmp(param,"middle")==0){
            xp_wheel_motorSpeed_set(200,200);
        }
        else if(strcmp(param,"right")==0){
            xp_wheel_motorSpeed_set(200,100);
        }
        else{
            return 0;
        }
    }
    else if(strcmp(fun,"get_encode")==0){
        PANS_encode encode={0};
        if(strcmp(param,"0")==0){
            xp_pans_get_encode(&wheel_handler[0],&encode);
            println("wheel0 encode: %d.%d  %d",encode.r,encode.p,encode.h);
        }
        else if(strcmp(param,"1")==0){
            xp_pans_get_encode(&wheel_handler[1],&encode);
            println("wheel1 encode: %d.%d  %d",encode.r,encode.p,encode.h);
        }
        else if(strcmp(param,"2")==0){
            xp_pans_get_encode(&wheel_handler[0],&encode);
            println("wheel0 encode: %d.%d  %d",encode.r,encode.p,encode.h);
            xp_pans_get_encode(&wheel_handler[1],&encode);
            println("wheel1 encode: %d.%d  %d",encode.r,encode.p,encode.h);
        }
    }
    else if(strcmp(fun,"locker_set")==0){
        xp_wheel_locker_set((WHEEL_locker_type)atoi(param));
    }
    else if(strcmp(fun,"locker_get")==0){
        println("wheel locker type: %s.",xp_lockerString_get(xp_wheel_locker_get()));
    }
	else{
        return 0;
    }
    return 1;
}

//end of the file
